import rclpy
from rclpy.node import Node
import tf2_ros
from geometry_msgs.msg import Twist
from std_msgs.msg import String
import numpy as np
class TF2Listener(Node):

    def __init__(self):
        super().__init__('op_start')
        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
        self.timer = self.create_timer(1.0, self.get_base_link_coordinates)
        self.subscription = self.create_subscription(String,'start_op',self.callback,10)
        self.subscription  # 防止被垃圾回收
        self.mysb_x=0
        self.mysb_y=0
        self.publisher = self.create_publisher(Twist, 'cmd_gimbal', 50)
    def callback(self, msg):
        x_y = msg.data.split(";")
        if len(x_y) >= 2:
            cmd_gimbal_value_ = Twist()
            cmd_gimbal_value_.angular.z=np.arctan2(float(x_y[1])-self.mysb_y,float(x_y[0])-self.mysb_x)/3.14*180+180
            self.publisher.publish(cmd_gimbal_value_)
        

    def get_base_link_coordinates(self):
        try:
            # 获取base_link相对于map坐标系的变换
            transform_stamped = self.tf_buffer.lookup_transform('map', 'base_link', rclpy.time.Time().to_msg())
            self.mysb_x=transform_stamped.transform.translation.x
            self.mysb_y=transform_stamped.transform.translation.y

            
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as ex:
            self.get_logger().warn('无法获取base_link坐标： %s' % str(ex))

def main(args=None):
    rclpy.init(args=args)
    tf2_listener = TF2Listener()
    rclpy.spin(tf2_listener)
    tf2_listener.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
